#include "Cr.h"
#include "CrMatrix4f.h"
#include "CrQuaternion.h"

#undef QMATH

namespace cr
{
	const Matrix4f Matrix4f::Identity(	Vec4f(1.0f,0.0f,0.0f,0.0f),
		Vec4f(0.0f,1.0f,0.0f,0.0f),
		Vec4f(0.0f,0.0f,1.0f,0.0f),
		Vec4f(0.0f,0.0f,0.0f,1.0f)	);


	const Matrix4f Matrix4f::IndexMatrix(	Vec4f(0.0f,1.0f,2.0f,3.0f),
		Vec4f(0.1f,1.1f,2.1f,3.1f),
		Vec4f(0.2f,1.2f,2.2f,3.2f),
		Vec4f(0.3f,1.3f,2.3f,3.3f)	);

	const Matrix4f Matrix4f::Zero(	Vec4f(0.0f,0.0f,0.0f,0.0f),
		Vec4f(0.0f,0.0f,0.0f,0.0f),
		Vec4f(0.0f,0.0f,0.0f,0.0f),
		Vec4f(0.0f,0.0f,0.0f,0.0f)	);

#ifdef _DEBUG
	bool Normalish(Vec4f p)
	{
		float l = p.lengthSquared3();
		return (l > 0.99f && l < 1.02f);	
	}
#endif

	Matrix4f::Matrix4f(Vec4f Direction, Vec4f Up)
	{

#if 0
#ifdef _DEBUG
		assert(Normalish(Direction));
		assert(Normalish(Up));
#endif
#endif
		m_Xaxis = Up.cross3_W0(Direction);
		m_Xaxis.normalise3();
		m_Yaxis = Direction.cross3_W0(m_Xaxis);
		m_Zaxis = m_Xaxis.cross3_W0(m_Yaxis);
		m_translation = Vec4f::Zero;

	}

	//	See Foley, Van Dam, et. al, p. 215	(remembering col<->row flip)

	Matrix4f Matrix4f::RotX(float angle)
	{
		float c=cosf(angle), s=sinf(angle);

		return Matrix4f(Identity.m_Xaxis,
			Vec4f(0.0f,+c,+s,0.0f),
			Vec4f(0.0f,-s,+c,0.0f),
			Identity.m_translation);
	}

	Matrix4f Matrix4f::RotY(float angle)
	{
		float c=cosf(angle), s=sinf(angle);

		return Matrix4f(Vec4f(+c,0.0f,-s,0.0f),
			Identity.m_Yaxis,
			Vec4f(s,0.0f,+c,0.0f),
			Identity.m_translation);
	}

	Matrix4f Matrix4f::RotZ(float angle)
	{
		float c=cosf(angle), s=sinf(angle);

		return Matrix4f(Vec4f(+c,+s,0.0f,0.0f),
			Vec4f(-s,+c,0.0f,0.0f),
			Identity.m_Zaxis,
			Identity.m_translation);
	}

	/***********************************************************/

	//	"Quick" versions of the above

	Matrix4f Matrix4f::QRotX(float angle)
	{
		float c=cosf(angle), s=sinf(angle);

		return Matrix4f(Identity.m_Xaxis,
			Vec4f(0.0f,+c,+s,0.0f),
			Vec4f(0.0f,-s,+c,0.0f),
			Identity.m_translation);
	}

	Matrix4f Matrix4f::QRotY(float angle)
	{
		float c=cosf(angle), s=sinf(angle);

		return Matrix4f(Vec4f(+c,0.0f,-s,0.0f),
			Identity.m_Yaxis,
			Vec4f(s,0.0f,+c,0.0f),
			Identity.m_translation);
	}

	Matrix4f Matrix4f::QRotZ(float angle)
	{
		float c=cosf(angle), s=sinf(angle);

		return Matrix4f(Vec4f(+c,+s,0.0f,0.0f),
			Vec4f(-s,+c,0.0f,0.0f),
			Identity.m_Zaxis,
			Identity.m_translation);
	}

	//	Simple Euler angles, applied in XYZ order

	Matrix4f::Matrix4f(float pitch, float heading, float roll)
	{
		Matrix4f xrot=RotX(pitch);  
		Matrix4f yrot=RotY(heading);
		Matrix4f zrot=RotZ(roll);   
		*this = zrot*yrot*xrot;
	}


	void Matrix4f::operator=(const Quaternion &q)
	{															
		m_Xaxis.setX(1.0f - 2.0f * ( q.j*q.j + q.k*q.k ));		
		m_Yaxis.setX(2.0f * ( q.i*q.j - q.r*q.k ));			 
		m_Zaxis.setX(2.0f * ( q.i*q.k + q.r*q.j ));
		m_translation.setX(0.0f);

		m_Xaxis.setY(2.0f * ( q.i*q.j + q.r*q.k ));
		m_Yaxis.setY(1.0f - 2.0f * ( q.i*q.i + q.k*q.k ));
		m_Zaxis.setY(2.0f * ( q.j*q.k - q.r*q.i ));
		m_translation.setY(0.0f);

		m_Xaxis.setZ(2.0f * ( q.i*q.k - q.r*q.j ));
		m_Yaxis.setZ(2.0f * ( q.j*q.k + q.r*q.i ));
		m_Zaxis.setZ(1.0f - 2.0f * ( q.i*q.i + q.j*q.j ));
		m_translation.setZ(0.0f);

		m_Xaxis.setW(0.0f);
		m_Yaxis.setW(0.0f);
		m_Zaxis.setW(0.0f);
		m_translation.setW(1.0f);
	}

	void Matrix4f::EqRotAxis(Vec4f axis, float angle)
	{
		CR_ASSERT(fabs(axis.lengthSquared3()-1.0f) < 0.0001f);

		float c,s,t;

		float x=axis.getX();
		float y=axis.getY();
		float z=axis.getZ();

#ifdef QMATH
		c = qcos(angle);
		s = qsin(angle);
#else
		c = cosf(angle);
		s = sinf(angle);
#endif

		t = 1-c;   

		m_Xaxis.setX(t*x*x + c);
		m_Xaxis.setY(t*x*y - s*z);
		m_Xaxis.setZ(t*x*z + s*y);
		m_Xaxis.setW(0.0f);

		m_Yaxis.setX(t*x*y + s*z);
		m_Yaxis.setY(t*y*y + c);
		m_Yaxis.setZ(t*y*z - s*x);
		m_Yaxis.setW(0.0f);

		m_Zaxis.setX(t*x*z - s*y);
		m_Zaxis.setY(t*y*z + s*x);
		m_Zaxis.setZ(t*z*z + c);
		m_Zaxis.setW(0.0f);

		m_translation=Vec4f(0.0f, 0.0f, 0.0f, 1.0f);
	}

	void Matrix4f::RotateAboutAxis(Vec4f &axis, float angle)
	{
		Matrix4f mat;
		mat.EqRotAxis(axis, angle);
		*this=mat**this;
	}

	Vec4f Matrix4f::transform(Vec4f vert) const
	{
		//// which plonker did this one then Jimbo????
		//return Vec4f(m_Xaxis.dot4(vert), m_Yaxis.dot4(vert), m_Zaxis.dot4(vert), m_translation.dot4(vert));
		return Vec4f(m_Xaxis*vert.getX()+ m_Yaxis*vert.getY() +  m_Zaxis*vert.getZ() + m_translation*vert.getW());
	}

	Vec4f Matrix4f::rotate(Vec4f vert) const
	{
		return (m_Xaxis * vert.getX() + m_Yaxis * vert.getY() + m_Zaxis * vert.getZ());
	}
}